
/**
 * @file filter.c
 * Audio filters
 */

#include "common.h"

#include <stdlib.h>
#include <string.h>
#include <assert.h>

#include "filter.h"

typedef struct Filter {
    const char *name;
    enum FilterID id;
    int private_size;
    int (*init)(FilterContext *f);
    void (*filter)(FilterContext *f, double *out, double *in, int n);
} Filter;


typedef struct {
    double coefs[5];
    double state[2][5];
} BiquadContext;

static void biquad_generate_lowpass(BiquadContext *f, double fc)
{
    double omega, alpha, cs;
    double a[3], b[3];

    omega = 2.0 * M_PI * fc;
    alpha = sin(omega) / 2.0;
    cs = cos(omega);

    a[0] =  1.0 + alpha;
    a[1] = -2.0 * cs;
    a[2] =  1.0 - alpha;
    b[0] = (1.0 - cs) / 2.0;
    b[1] = (1.0 - cs);
    b[2] = (1.0 - cs) / 2.0;

    f->coefs[0] = b[0] / a[0];
    f->coefs[1] = b[1] / a[0];
    f->coefs[2] = b[2] / a[0];
    f->coefs[3] = a[1] / a[0];
    f->coefs[4] = a[2] / a[0];
}

static void biquad_generate_highpass(BiquadContext *f, double fc)
{
    double omega, alpha, cs;
    double a[3], b[3];

    omega = 2.0 * M_PI * fc;
    alpha = sin(omega) / 2.0;
    cs = cos(omega);

    a[0] =   1.0 + alpha;
    a[1] =  -2.0 * cs;
    a[2] =   1.0 - alpha;
    b[0] =  (1.0 + cs) / 2.0;
    b[1] = -(1.0 + cs);
    b[2] =  (1.0 + cs) / 2.0;

    f->coefs[0] = b[0] / a[0];
    f->coefs[1] = b[1] / a[0];
    f->coefs[2] = b[2] / a[0];
    f->coefs[3] = a[1] / a[0];
    f->coefs[4] = a[2] / a[0];
}

static int biquad_init(FilterContext *f)
{
    int i, j;
    BiquadContext *b = f->private_context;
    double fc;

    if (f->samplerate <= 0)
        return -1;
    if (f->cutoff < 0 || f->cutoff > (f->samplerate/2.0))
        return -1;
    fc = f->cutoff / f->samplerate;

    switch (f->type) {
        case FILTER_TYPE_LOWPASS:
            biquad_generate_lowpass(b, fc);
            break;
        case FILTER_TYPE_HIGHPASS:
            biquad_generate_highpass(b, fc);
            break;
        default:
            return -1;
    }

    for (j = 0; j < 2; j++) {
        for (i = 0; i < 5; i++)
            b->state[j][i] = 0.0;
    }

    return 0;
}

static void biquad_i_run_filter(FilterContext *f, double *out, double *in, int n)
{
    BiquadContext *b = f->private_context;
    double *coefs = b->coefs;
    double *tmp = in;
    int i, j;

    for (j = 0; j < 1+f->cascaded; j++) {
        double *state_j = b->state[j];
        for (i = 0; i < n; i++) {
            double v = 0;
            state_j[0] = tmp[i];

            v += coefs[0] * state_j[0];
            v += coefs[1] * state_j[1];
            v += coefs[2] * state_j[2];
            v -= coefs[3] * state_j[3];
            v -= coefs[4] * state_j[4];

            state_j[2] = state_j[1];
            state_j[4] = state_j[3];
            state_j[1] = state_j[0];
            state_j[3] = v;

            out[i] = CLIP(v, -1.0, 1.0);
        }
        tmp = out;
    }
}

static void biquad_ii_run_filter(FilterContext *f, double *out, double *in, int n)
{
    BiquadContext *b = f->private_context;
    double *coefs = b->coefs;
    double *tmp = in;
    int i, j;
    double v;

    for (j = 0; j < 1+f->cascaded; j++) {
        double *state_j = b->state[j];
        for (i = 0; i < n; i++) {
            state_j[0] = tmp[i];

            v = coefs[0] * state_j[0] + state_j[1];
            state_j[1] = coefs[1] * state_j[0] - coefs[3] * v + state_j[2];
            state_j[2] = coefs[2] * state_j[0] - coefs[4] * v;

            out[i] = CLIP(v, -1.0, 1.0);
        }
        tmp = out;
    }
}

static const Filter biquad_i_filter = {
    "Biquad Direct Form I",
    FILTER_ID_BIQUAD_I,
    sizeof(BiquadContext),
    biquad_init,
    biquad_i_run_filter,
};

static const Filter biquad_ii_filter = {
    "Biquad Direct Form II",
    FILTER_ID_BIQUAD_II,
    sizeof(BiquadContext),
    biquad_init,
    biquad_ii_run_filter,
};

static void butterworth_generate_lowpass(BiquadContext *f, double fc)
{
    double c = 1.0 / tan(M_PI * fc);
    double c2 = (c * c);

    f->coefs[0] = 1.0 / (c2 + M_SQRT2 * c + 1.0);
    f->coefs[1] = 2.0 * f->coefs[0];
    f->coefs[2] = f->coefs[0];
    f->coefs[3] = 2.0 * (1.0 - c2) * f->coefs[0];
    f->coefs[4] = (c2 - M_SQRT2 * c + 1.0) * f->coefs[0];
}

static void butterworth_generate_highpass(BiquadContext *f, double fc)
{
    double c = tan(M_PI * fc);
    double c2 = (c * c);

    f->coefs[0] = 1.0 / (c2 + M_SQRT2 * c + 1.0);
    f->coefs[1] = -2.0 * f->coefs[0];
    f->coefs[2] = f->coefs[0];
    f->coefs[3] = 2.0 * (c2 - 1.0) * f->coefs[0];
    f->coefs[4] = (c2 - M_SQRT2 * c + 1.0) * f->coefs[0];
}

static int butterworth_init(FilterContext *f)
{
    int i, j;
    BiquadContext *b = f->private_context;
    double fc;

    if (f->samplerate <= 0)
        return -1;
    if (f->cutoff < 0 || f->cutoff > (f->samplerate/2.0))
        return -1;
    fc = f->cutoff / f->samplerate;

    switch (f->type) {
        case FILTER_TYPE_LOWPASS:
            butterworth_generate_lowpass(b, fc);
            break;
        case FILTER_TYPE_HIGHPASS:
            butterworth_generate_highpass(b, fc);
            break;
        default:
            return -1;
    }

    for (j = 0; j < 2; j++) {
        for (i = 0; i < 5; i++)
            b->state[j][i] = 0.0;
    }

    return 0;
}

static const Filter butterworth_i_filter = {
    "Butterworth Direct Form I",
    FILTER_ID_BUTTERWORTH_I,
    sizeof(BiquadContext),
    butterworth_init,
    biquad_i_run_filter,
};

static const Filter butterworth_ii_filter = {
    "Butterworth Direct Form II",
    FILTER_ID_BUTTERWORTH_II,
    sizeof(BiquadContext),
    butterworth_init,
    biquad_ii_run_filter,
};


/* One-pole, One-zero filter */

typedef struct {
    double p;
    double last;
} OnePoleContext;

static void onepole_generate_lowpass(OnePoleContext *o, double fc)
{
    double omega, cs;

    omega = 2.0 * M_PI * fc;
    cs = 2.0 - cos(omega);
    o->p = cs - sqrt((cs*cs)-1.0);
    o->last = 0.0;
}

static void onepole_generate_highpass(OnePoleContext *o, double fc)
{
    double omega, cs;

    omega = 2.0 * M_PI * fc;
    cs = 2.0 + cos(omega);
    o->p = cs - sqrt((cs*cs)-1.0);
    o->last = 0.0;
}

static int onepole_init(FilterContext *f)
{
    OnePoleContext *o = f->private_context;
    double fc;

    if (f->cascaded)
        return -1;
    if (f->samplerate <= 0)
        return -1;
    if (f->cutoff < 0 || f->cutoff > (f->samplerate/2.0))
        return -1;
    fc = f->cutoff / f->samplerate;

    switch (f->type) {
        case FILTER_TYPE_LOWPASS:
            onepole_generate_lowpass(o, fc);
            break;
        case FILTER_TYPE_HIGHPASS:
            onepole_generate_highpass(o, fc);
            break;
        default:
            return -1;
    }

    return 0;
}

static void onepole_run_filter(FilterContext *f, double *out, double *in, int n)
{
    int i;
    double v;
    double p1 = 0;
    OnePoleContext *o = f->private_context;

    if (f->type == FILTER_TYPE_LOWPASS)
        p1 = 1.0 - o->p;
    else if (f->type == FILTER_TYPE_HIGHPASS)
        p1 = o->p - 1.0;

    for (i = 0; i < n; i++) {
        v = (p1 * in[i]) + (o->p * o->last);
        o->last = out[i] = CLIP(v, -1.0, 1.0);
    }
}

static const Filter onepole_filter = {
    "One-Pole Filter",
    FILTER_ID_ONEPOLE,
    sizeof(OnePoleContext),
    onepole_init,
    onepole_run_filter,
};


int a52_filter_init(FilterContext *f, enum FilterID id)
{
    assert(f != NULL);

    switch (id) {
        case FILTER_ID_BIQUAD_I:
            f->filter = &biquad_i_filter;
            break;
        case FILTER_ID_BIQUAD_II:
            f->filter = &biquad_ii_filter;
            break;
        case FILTER_ID_BUTTERWORTH_I:
            f->filter = &butterworth_i_filter;
            break;
        case FILTER_ID_BUTTERWORTH_II:
            f->filter = &butterworth_ii_filter;
            break;
        case FILTER_ID_ONEPOLE:
            f->filter = &onepole_filter;
            break;
        default:
            return -1;
    }

    f->private_context = calloc(f->filter->private_size, 1);

    return f->filter->init(f);
}

void a52_filter_run(FilterContext *f, double *out, double *in, int n)
{
    assert(f != NULL && f->filter != NULL && f->filter->filter != NULL);
    assert(out != NULL && in != NULL);
    assert(n >= 0);
    f->filter->filter(f, out, in, n);
}

void a52_filter_close(FilterContext *f)
{
    assert(f != NULL);
    if (f->private_context) {
        free(f->private_context);
        f->private_context = NULL;
    }
    f->filter = NULL;
}
